#include "common/common.h"
#include "sharpir.h"
#include "beep.h"
#include "utils/filters.h"

enum {
    IR_ADC_STATE_IDLE = 0,
    IR_ADC_STATE_WAITING = 1,
};
#define PWR_FILTER_QUEUE_BIT    3
#define PWR_FILTER_QUEUE_SIZE   (0x1<<PWR_FILTER_QUEUE_BIT)


typedef struct _adcSamplePair {
    _u16 bitPos;
    _u16 adcPin;
    _s16 distance;    
} adcSamplePair_t;

static _u16 _pwrCachedBattVoltADCVal = 0;
static _u16 _pwrFilterBattVoltQueue[PWR_FILTER_QUEUE_SIZE+2];
static _u8  _pwrAdcSampleState = 0;

static _u32 irFrequency = 0;

static adcSamplePair_t _adcSamplePairs[] = {
    {IRSENSOR_BOTTOM_R1_BIT, BOTTOM_IR_R1_ADC_CH},
    {IRSENSOR_BOTTOM_R2_BIT, BOTTOM_IR_R2_ADC_CH},
    {IRSENSOR_BOTTOM_R3_BIT, BOTTOM_IR_R3_ADC_CH},
    {IRSENSOR_BOTTOM_R4_BIT, BOTTOM_IR_R4_ADC_CH},
};


static void _sharpir_sample_distance();

/*
 * 获取电池电压函数
 * 返回电压值，单位：mV
 */
_u32 get_distance(void)
{
    // const _u32  ADC_LEVELS = (0x1<<ADC_RES_BIT); //4096
  //  ADC_REF = 2.495
  //  VBATT / BATTERY_VOLTAGE_SCALEFACTOR = adc_val * ADC_REF / 4096
  // const float ADC_TO_BATT_VOLT_FACTOR = (ADC_REF_VOLT * 1000 * BATTERY_VOLTAGE_SCALEFACTOR) / ADC_LEVELS;
  // const _u32  ADC_TO_BATT_VOLT_FACTOR_fixQ10 = (_u32)(ADC_TO_BATT_VOLT_FACTOR * 1024.0);
  // return (_pwrCachedBattVoltADCVal * ADC_TO_BATT_VOLT_FACTOR_fixQ10)>>10;

    // DBG_OUT("recvData.sensor_dis.distance[ch] = %d\r\n", recvData.sensor_dis.distance[ch]);
    int distanceCM = 27.728 * pow(_pwrCachedBattVoltADCVal/1000.0, -1.2045);
    // distanceCM = distanceCM < 30 ? distanceCM : 1000;
    return (uint32_t)(distanceCM * 10);

}

static void _on_battery_adc_data_ready(_u16 adcData)
{
    _pwrCachedBattVoltADCVal = add_to_avg_filter_u16(adcData, _pwrFilterBattVoltQueue, _pwrFilterPos, PWR_FILTER_QUEUE_SIZE);
    if ( (++_pwrFilterPos) >= PWR_FILTER_QUEUE_SIZE) {
        _pwrFilterPos = 0;
    }
}

static void _sharpir_sample_distance()
{
    switch (_pwrAdcSampleState) {
    case BATTERY_ADC_STATE_IDLE:
        adc_read_start(GET_ADC(BATTERY_VOLTAGE_SAMPLE_ADC_ID), BATTERY_VOLTAGE_SAMPLE_CHANNEL);
        _pwrAdcSampleState = BATTERY_ADC_STATE_WAITING;
        break;
    case BATTERY_ADC_STATE_WAITING:
        if (adc_read_is_ready(GET_ADC(BATTERY_VOLTAGE_SAMPLE_ADC_ID))) {
            _on_battery_adc_data_ready(adc_read_final(GET_ADC(BATTERY_VOLTAGE_SAMPLE_ADC_ID)));
            _pwrAdcSampleState = BATTERY_ADC_STATE_IDLE;
        }
        break;
    }
}

/*
 * 电池相关初始化函数
 */
void init_sharpir(void)
{
    _pwrAdcSampleState = BATTERY_ADC_STATE_IDLE;
    _pwrFilterPos = 0;
    // preheat the adc avg-filter queue...
    do{
          _sharpir_sample_distance();
      } while (_pwrFilterPos!=0);
}
/*
 * 电池相关模块函数
 * 充电状态的判定等
 */
void heartbeat_battery(void)
{
    _sharpir_sample_distance();
    if ((getms() - irFrequency) >= 20) {
        //20毫秒秒检测一次
        irFrequency = getms();

        for (int i = 0; i < sizeof(_adcSamplePairs); ++i)
        {
        	_adcSamplePairs[i].distance = get_distance();
        }
        _u32 currentVolt = get_distance();
        // DBG_OUT("currentVolt = %d\r\n", currentVolt);
        if (currentVolt < BATTERY_VOLTAGE_EMPTY) {
            batteryElectricityPercentage = 0;
        } else if (currentVolt > BATTERY_VOLTAGE_FULL) {
            batteryElectricityPercentage = 100;
        } else {
            batteryElectricityPercentage = (currentVolt - BATTERY_VOLTAGE_EMPTY)*100 / (BATTERY_VOLTAGE_FULL - BATTERY_VOLTAGE_EMPTY);
        }

        if (batteryElectricityPercentage < 25 && ISCHARGE_CHRG != charge_detect_getstatus()) {
            {

//                beep_beeper(3000, 400, chargeSound);
                if ((chargeSound += 1) >= 250) {
                    chargeSound = 250;
                }
            }
        } else {
            chargeSound = 2;
        }
    }
    if (isChargeInserted) {
        //是否处在充电桩充电状态
        if (!PIN_READ(GPIOB, HOCHARGE_DETECT)) {
            //在充电桩充电下：检测是否脱离充电桩，拔出则更改充电桩充电状态
            if (getms() - chargeInsertedTs >= CONFIG_CHARGEBASE_REMOVAL_THRESHOLDTS) {
//                beep_beeper(5000, 80, 2);
                isChargeInserted = 0;
            }
        } else {
            chargeInsertedTs = getms();
        }

    } else {
        //不在充电桩充电下：检测是否脱离充电桩，插入则更改充电桩充电状态
        if (PIN_READ(GPIOB, HOCHARGE_DETECT)) {
            isChargeInserted = 1;
//            beep_beeper(5000, 80, 2);
            chargeInsertedTs = getms();
        }

    }
    if (isDcInserted) {
        //是否处在DC电源充电状态
        if (!PIN_READ(GPIOB, DCCHARGE_DETECT)) {
            //在DC电源充电下：检测是否拔出DC电源，拔出则更改DC电源充电状态
            if (getms() - dcInsertedTs >= CONFIG_CHARGEBASE_REMOVAL_THRESHOLDTS * 2) {
                //防抖动
                isDcInserted = 0;
            }
        } else {
            dcInsertedTs = getms();
        }

    } else {
        //不在DC电源充电下：检测DC电源是否插入，插入则更改DC电源充电状态
        if (PIN_READ(GPIOB, DCCHARGE_DETECT)) {
            isDcInserted = 1;
            dcInsertedTs = getms();
        }
    }
}
